#include "drv_algorithm.h"

#include "math.h"
#include "stdint.h"

#define NEWTON_MAX_ITER 100
#define NEWTON_TOL      1e-4f

/**
 * @brief 牛顿迭代法
 */
NewtonStatus_t ALGO_NewtonMethod(FuncWithCtx_t f, FuncWithCtx_t df, float x0, void *ctx, float *res)
{
    float x = x0;

    for (int16_t i = 0; i < NEWTON_MAX_ITER; i++)
    {
        float fx = f(x, ctx);
        float dfx = df(x, ctx);

        if (fabsf(dfx) < 1e-6f)
        {
            return NEWTON_DERIV_ZERO;
        }

        float x_new = x - fx / dfx;

        if (fabsf(x_new - x) < NEWTON_TOL)
        {
            *res = x_new;
            return NEWTON_SUCCESS;
        }

        x = x_new;
    }

    *res = x;
    return NEWTON_NO_CONVERGE;
}

/**
 * @brief 角度制转弧度制
 */
float ALGO_Deg2Rad(const float deg)
{
    return deg / 180.0 * M_PI;
}

/**
 * @brief 弧度制转角度制
 */
float ALGO_Rad2Deg(const float rad)
{
    return rad / M_PI * 180.0;
}

/**
 * @brief 将角度限制到[-π, π]
 */
float ALGO_WrapToPi(const float rad)
{
    float wrap = fmodf(rad, 2 * M_PI);

    if (wrap < -M_PI)
    {
        wrap += 2 * M_PI;
    }
    else if (wrap > M_PI)
    {
        wrap -= 2 * M_PI;
    }

    return wrap;
}

/**
 * @brief 将角度限制到[-180°, 180°]
 */
float ALGO_WrapTo180(const float deg)
{
    float wrap = fmodf(deg, 2 * 180.0);

    if (wrap < -180.0)
    {
        wrap += 2 * 180.0;
    }
    else if (wrap > 180.0)
    {
        wrap -= 2 * 180.0;
    }

    return wrap;
}
